#include "ros/ros.h"
#include "std_msgs/Bool.h"
#include "std_msgs/String.h"
#include "std_msgs/Time.h"
#include <algorithm>
#include <memory>
using namespace std;

/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */
void chatterCallback(const std_msgs::String::ConstPtr &msg) {
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

class NormalTimeExpire {
public:
  ros::Time t;
  ros::NodeHandle n;
  ros::Publisher time_pub;
  ros::Timer timer;
  string name;

  void timerCallback(const ros::TimerEvent &e) {
    ROS_INFO_STREAM(this<<"  in timerCallback");
    std_msgs::Time time;
    time.data = t;
    time_pub.publish(time);
    ros::spinOnce();
  };
  
  NormalTimeExpire(ros::NodeHandle n,string name) {
    this->n = n;
    this->name = name;
    time_pub = n.advertise<std_msgs::Time>(name+"/NormalTimeExpiretopic", 1);

    t = ros::Time::now();
    timer = n.createTimer(ros::Duration(10), &NormalTimeExpire::timerCallback, this,
                          true);
  }

  ~NormalTimeExpire() {
    // ROS_INFO_STREAM("  in ~NormalTimeExpire");
  }
};

typedef shared_ptr<NormalTimeExpire> NormalTimeExpirePtr;

class NormalTimeMgr {
public:
  // vector<pair<NormalTimeExpirePtr, ros::Subscriber>> arr;
  ros::NodeHandle n;
  string name;

  ros::Publisher count_pub;

  int iThreshold = 10;
  int iThresholdSelf = 10;
  vector<NormalTimeExpirePtr> arr;
  vector<ros::Subscriber> subArr;
  ros::Subscriber sub;


  NormalTimeMgr(ros::NodeHandle n,string name) {
    this->n = n;
    this->name = name;
    
    sub=n.subscribe(name+"/NormalTimeExpiretopic", 1000, &NormalTimeMgr::msgtimedelCallback,
                        this);


    count_pub = n.advertise<std_msgs::Bool>(name+"/threshold", 10);
  }
  void msgCallback(const std_msgs::Bool::ConstPtr &msg) {
    ROS_INFO_STREAM(name+"  in msgCallback");
    NormalTimeExpirePtr te = make_shared<NormalTimeExpire>(n,name);
    // pair<NormalTimeExpirePtr, ros::Subscriber> p(
    //     te, n.subscribe("NormalTimeExpiretopic", 1000, &NormalTimeMgr::msgtimedelCallback,
    //                     this));
    // arr.push_back(pair<NormalTimeExpirePtr, ros::Subscriber>(
    //     te, n.subscribe("NormalTimeExpiretopic", 1000, &NormalTimeMgr::msgtimedelCallback,
    //                     this)));

    arr.push_back(te);

    // subArr.push_back(n.subscribe("NormalTimeExpiretopic", 1000, &NormalTimeMgr::msgtimedelCallback,
    //                     this)) ;
    ROS_INFO_STREAM("arr size:" << arr.size());

    if (arr.size() > iThreshold) {
      std_msgs::Bool count_flag;
      count_flag.data = true;
      count_pub.publish(count_flag);
      ros::spinOnce();
    }
    // ros::Subscriber sub = n.subscribe("NormalTimeExpire", 1000,
    // &NormalTimeMgr::msgtimedelCallback, this);
    // subArr.push_back();
  }
  void msgtimedelCallback(const std_msgs::Time::ConstPtr &msg) {
    ROS_INFO_STREAM(name+"  in msgtimedelCallback");


    bool flag = false;
    auto iter = arr.begin();
    for (; iter != arr.end(); iter++) {
      assert(*iter != nullptr);
      if ((*iter)->t == msg->data) {
        flag = true;
        break;
      }
    }

    if (flag) {
      arr.erase(iter);
      ROS_INFO_STREAM(name+"  arr size:" << arr.size());

      if (arr.size() > iThreshold) {
        std_msgs::Bool count_flag;
        count_flag.data = true;
        count_pub.publish(count_flag);
        ros::spinOnce();
      }
    }
    else{
      ROS_INFO_STREAM(name+"uuuuuuuuuuuuuuuuuuuuu:" << arr.size());

    }
  }
};



int main(int argc, char *argv[]) {

  ros::init(argc, argv, "countinperiod_sub");

  ros::NodeHandle n;

  NormalTimeMgr t(n, "origin");

  ros::Subscriber sub =
      n.subscribe("chatter", 1000, &NormalTimeMgr::msgCallback, &t);

  // FinalTimeMgr t_self(n, "final");

  // ros::NodeHandle n1;
  // ros::Subscriber sub_self =
  //     n1.subscribe("origin/threshold", 1000, &FinalTimeMgr::msgCallback, &t_self);

  ros::spin();

  return 0;
}